#include <rov/sensors.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <pthread.h>

#include <iostream>


using namespace std;


#define BUFFER_SIZE	127
#define PORT		32001
#define BACKLOG		1


typedef struct
{
    Sensor imu;
    volatile int fd[2];
    volatile struct sockaddr_in addr[2];
}
Conn;


int callback(Conn * client, char * buf, int len)
{
    if (client->fd[0] >= 0)
    {
        int n = sendto(client->fd[0], buf, len, 0, (struct sockaddr *) &client->addr[0], sizeof(client->addr[0]));

//cerr << ">";

        if (n < 0)
        {
            printf("error: connection closed\n");
            close(client->fd[0]);
            client->fd[0] = -1;
        }
    }

//buf[len] = 0;
//cerr << "imu: \"" << buf << "\"";

    return 0;
}


void * senderThreadEntry(void * conn)
{
    char buf[BUFFER_SIZE + 1];
    Conn * client = (Conn *) conn;

    int status = loopDataGather<Conn*>(callback, client->imu, buf, BUFFER_SIZE, '\n', client);

    if (status != 0)
        cerr << "error reading imu data\n";

    return NULL;
}


int main(int argc, char ** argv)
{
    int listenfd;
    struct sockaddr_in servaddr;
    socklen_t clilen;
    Conn client;

    client.imu = initImu();
    client.fd[0] = -1;
    client.fd[1] = -1;

    printf("imu status: %d\n", client.imu);

    pthread_t sender;

    pthread_create(&sender, NULL, senderThreadEntry, &client);

    listenfd = socket(AF_INET, SOCK_STREAM, 0);

    bzero(&servaddr, sizeof(servaddr));

    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(PORT);

    bind(listenfd, (struct sockaddr *) &servaddr, sizeof(servaddr));

    listen(listenfd, BACKLOG);

    for(;;)
    {
        clilen = sizeof(client.addr[0]);
        client.fd[0] = accept(listenfd, (struct sockaddr *) &client.addr[0], &clilen);

cout << "new client\n";

        while (client.fd[0] >= 0);
    }

    close(listenfd);

    return 0;
}
